import roslib; roslib.load_manifest('framer')
import rospy
import actionlib
import smach
from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction, MoveBaseActionFeedback
from actionlib_msgs.msg import GoalStatus
from framer.msg import Status

class MoveBase(smach.State):
    def __init__(self):
        smach.State.__init__(self,
            outcomes    = ['fail', 'success', 'abort'],
            input_keys  = ['base_goal', 'picture_center'],
            output_keys = ['picture_center'],
        )
        rospy.loginfo("Creating base movement client.")
        self.baseClient = actionlib.SimpleActionClient(
            'move_base',
            MoveBaseAction
        )
        self.baseClient.wait_for_server()
        rospy.loginfo("Base client initialized")
        self.status_pub = rospy.Publisher('/framer/status', Status)

    def execute(self, data):
        self.status_pub.publish(Status(Status.MOVING_BODY))
        rospy.loginfo("Moving the base.")
        if rospy.is_shutdown(): # Exiting gracefully when ctrl-c is pressed
            return 'abort'
        self.baseClient.send_goal(data.base_goal)
        self.baseClient.wait_for_result()

        if self.baseClient.get_state() == GoalStatus.SUCCEEDED:
            return 'success'
        rospy.logerr("Expected success, got: %s" % self.baseClient.get_state())
        return 'fail'
